#include "pathological.h"


// Component wavelengths are random, but intensities == intensity
inline void random_gray(color_t *c, float intensity)
{
	int i;
	for(i = 0; i < SPEC_SAMPLES; i++)
		c->w[i] = SPEC_START
			+ (i + ran())*(SPEC_END - SPEC_START)/SPEC_SAMPLES,
		c->i[i] = intensity;
}

inline void color_copy(color_t *dest, color_t *src)
{
	int i;
	for(i = 0; i < SPEC_SAMPLES; i++)
		dest->w[i] = src->w[i], dest->i[i] = src->i[i];
}

// Linear interpolation
inline float color_sample(color_t *c, float lambda)
{
	int bin = floor((lambda - SPEC_START)/SPEC_STEP);
	if(lambda < c->w[bin])
		if(bin == 0) return c->i[0];
		else bin--;
	else if(bin == SPEC_SAMPLES - 1) return c->i[SPEC_SAMPLES - 1];
	return c->i[bin] + (c->i[bin + 1] - c->i[bin])*(lambda - c->w[bin])
		/(c->w[bin + 1] - c->w[bin]);
}

// All color math assumes the wavelengths in each color are the same

// a += b
inline void color_add(color_t *a, color_t *b)
{
#if X86_ASM && SPEC_SAMPLES == 8
	asm("movups (%0), %%xmm0;"
		"movups (%1), %%xmm1;"
		"addps %%xmm1, %%xmm0;"
		"movups %%xmm0, (%0);"
		"movups 16(%0), %%xmm0;"
		"movups 16(%1), %%xmm1;"
		"addps %%xmm1, %%xmm0;"
		"movups %%xmm0, 16(%0);"
		::"r"(a->i),"r"(b->i));
#else
	int i;
	for(i = 0; i < SPEC_SAMPLES; i++) a->i[i] += b->i[i];
#endif
}

// a -= b
inline void color_sub(color_t *a, color_t *b)
{
#if X86_ASM && SPEC_SAMPLES == 8
	asm("movups (%0), %%xmm0;"
		"movups (%1), %%xmm1;"
		"subps %%xmm1, %%xmm0;"
		"movups %%xmm0, (%0);"
		"movups 16(%0), %%xmm0;"
		"movups 16(%1), %%xmm1;"
		"subps %%xmm1, %%xmm0;"
		"movups %%xmm0, 16(%0);"
		::"r"(a->i),"r"(b->i));
#else
	int i;
	for(i = 0; i < SPEC_SAMPLES; i++) a->i[i] -= b->i[i];
#endif
}

// a *= b
inline void color_mul(color_t *a, color_t *b)
{
#if X86_ASM && SPEC_SAMPLES == 8
	asm("movups (%0), %%xmm0;"
		"movups (%1), %%xmm1;"
		"mulps %%xmm1, %%xmm0;"
		"movups %%xmm0, (%0);"
		"movups 16(%0), %%xmm0;"
		"movups 16(%1), %%xmm1;"
		"mulps %%xmm1, %%xmm0;"
		"movups %%xmm0, 16(%0);"
		::"r"(a->i),"r"(b->i));
#else
	int i;
	for(i = 0; i < SPEC_SAMPLES; i++) a->i[i] *= b->i[i];
#endif
}

// c *= s
inline void color_scale(color_t *c, float s)
{
#if X86_ASM && SPEC_SAMPLES == 8
	asm("movss %1, %%xmm1;"
		"shufps $0b00000000, %%xmm1, %%xmm1;"
		"movups (%0), %%xmm0;"
		"mulps %%xmm1, %%xmm0;"
		"movups %%xmm0, (%0);"
		"movups 16(%0), %%xmm0;"
		"mulps %%xmm1, %%xmm0;"
		"movups %%xmm0, 16(%0);"
		::"r"(c->i),"m"(s));
#else
	int i;
	for(i = 0; i < SPEC_SAMPLES; i++) c->i[i] *= s;
#endif
}

// c = exp(c)
inline void color_exp(color_t *c)
{
	int i;
	for(i = 0; i < SPEC_SAMPLES; i++)
		c->i[i] = c->i[i] > FLT_MAX
			? FLT_MAX
			: c->i[i] < -FLT_MAX
			? 0
			: exp(c->i[i]);
}

// c = 1/c
inline void color_rcp(color_t *c)
{
	int i;
	for(i = 0; i < SPEC_SAMPLES; i++) c->i[i] = 1/c->i[i];
}

int spectrum_sample_compare(const void *a,
	const void *b)
{
	if(((spectrum_sample_t *)a)->w < ((spectrum_sample_t *)b)->w)
		return -1;
	else if(((spectrum_sample_t *)a)->w == ((spectrum_sample_t *)b)->w)
		return 0;
	else
		return 1;
}

void resample_spectrum(color_t *c, spectrum_t *s)
{
	int i, j;
	float x1, x2, m, i1, i2;

	qsort(s->samples,s->numsamples,sizeof(spectrum_sample_t),
		spectrum_sample_compare);

	for(i = 0; i < SPEC_SAMPLES; i++)
		c->w[i] = SPEC_START + i*SPEC_STEP, c->i[i] = 0;

	for(i = 0; i < s->numsamples - 1; i++)
	{
		for(j = MAX((s->samples[i].w - SPEC_START)/SPEC_STEP,0);
			j < SPEC_SAMPLES && j < (s->samples[i+1].w - SPEC_START)/SPEC_STEP;
			j++)
		{
			x1 = MAX(c->w[j] - s->samples[i].w,0);
			x2 = MIN(s->samples[i+1].w,c->w[j] + SPEC_STEP) - s->samples[i].w;
			m = (s->samples[i+1].i - s->samples[i].i)
				/(s->samples[i+1].w - s->samples[i].w);
			i1 = s->samples[i].i + x1*m, i2 = s->samples[i].i + x2*m;
			c->i[j] += (i1 + i2)*(x2 - x1)/2;
		}
	}

	color_scale(c,1.0/SPEC_STEP);
}


// CIE color-matching function stuff
// RGB is in terms of sRGB

// The 1931 CIE standard observer [xyz]-bar
#define CIEXYZ_START 380
#define CIEXYZ_SAMPLES 81
#define CIEXYZ_END 780
#define CIEXYZ_STEP ((CIEXYZ_END - CIEXYZ_START)/(CIEXYZ_SAMPLES - 1.0))
const float cieXYZ[CIEXYZ_SAMPLES][3] = {
	{0.001368,0.000039,0.006450}, // 380 nm
	{0.002236,0.000064,0.010550}, // 385 nm
	{0.004243,0.000120,0.020050}, // 390 nm
	{0.007650,0.000217,0.036210}, // 395 nm
	{0.014310,0.000396,0.067850}, // 400 nm
	{0.023190,0.000640,0.110200}, // 405 nm
	{0.043510,0.001210,0.207400}, // 410 nm
	{0.077630,0.002180,0.371300}, // 415 nm
	{0.134380,0.004000,0.645600}, // 420 nm
	{0.214770,0.007300,1.039050}, // 425 nm
	{0.283900,0.011600,1.385600}, // 430 nm
	{0.328500,0.016840,1.622960}, // 435 nm
	{0.348280,0.023000,1.747060}, // 440 nm
	{0.348060,0.029800,1.782600}, // 445 nm
	{0.336200,0.038000,1.772110}, // 450 nm
	{0.318700,0.048000,1.744100}, // 455 nm
	{0.290800,0.060000,1.669200}, // 460 nm
	{0.251100,0.073900,1.528100}, // 465 nm
	{0.195360,0.090980,1.287640}, // 470 nm
	{0.142100,0.112600,1.041900}, // 475 nm
	{0.095640,0.139020,0.812950}, // 480 nm
	{0.057950,0.169300,0.616200}, // 485 nm
	{0.032010,0.208020,0.465180}, // 490 nm
	{0.014700,0.258600,0.353300}, // 495 nm
	{0.004900,0.323000,0.272000}, // 500 nm
	{0.002400,0.407300,0.212300}, // 505 nm
	{0.009300,0.503000,0.158200}, // 510 nm
	{0.029100,0.608200,0.111700}, // 515 nm
	{0.063270,0.710000,0.078250}, // 520 nm
	{0.109600,0.793200,0.057250}, // 525 nm
	{0.165500,0.862000,0.042160}, // 530 nm
	{0.225750,0.914850,0.029840}, // 535 nm
	{0.290400,0.954000,0.020300}, // 540 nm
	{0.359700,0.980300,0.013400}, // 545 nm
	{0.433450,0.994950,0.008750}, // 550 nm
	{0.512050,1.000000,0.005750}, // 555 nm
	{0.594500,0.995000,0.003900}, // 560 nm
	{0.678400,0.978600,0.002750}, // 565 nm
	{0.762100,0.952000,0.002100}, // 570 nm
	{0.842500,0.915400,0.001800}, // 575 nm
	{0.916300,0.870000,0.001650}, // 580 nm
	{0.978600,0.816300,0.001400}, // 585 nm
	{1.026300,0.757000,0.001100}, // 590 nm
	{1.056700,0.694900,0.001000}, // 595 nm
	{1.062200,0.631000,0.000800}, // 600 nm
	{1.045600,0.566800,0.000600}, // 605 nm
	{1.002600,0.503000,0.000340}, // 610 nm
	{0.938400,0.441200,0.000240}, // 615 nm
	{0.854450,0.381000,0.000190}, // 620 nm
	{0.751400,0.321000,0.000100}, // 625 nm
	{0.642400,0.265000,0.000050}, // 630 nm
	{0.541900,0.217000,0.000030}, // 635 nm
	{0.447900,0.175000,0.000020}, // 640 nm
	{0.360800,0.138200,0.000010}, // 645 nm
	{0.283500,0.107000,0.000000}, // 650 nm
	{0.218700,0.081600,0.000000}, // 655 nm
	{0.164900,0.061000,0.000000}, // 660 nm
	{0.121200,0.044580,0.000000}, // 665 nm
	{0.087400,0.032000,0.000000}, // 670 nm
	{0.063600,0.023200,0.000000}, // 675 nm
	{0.046770,0.017000,0.000000}, // 680 nm
	{0.032900,0.011920,0.000000}, // 685 nm
	{0.022700,0.008210,0.000000}, // 690 nm
	{0.015840,0.005723,0.000000}, // 695 nm
	{0.011359,0.004102,0.000000}, // 700 nm
	{0.008111,0.002929,0.000000}, // 705 nm
	{0.005790,0.002091,0.000000}, // 710 nm
	{0.004109,0.001484,0.000000}, // 715 nm
	{0.002899,0.001047,0.000000}, // 720 nm
	{0.002049,0.000740,0.000000}, // 725 nm
	{0.001440,0.000520,0.000000}, // 730 nm
	{0.001000,0.000361,0.000000}, // 735 nm
	{0.000690,0.000249,0.000000}, // 740 nm
	{0.000476,0.000172,0.000000}, // 745 nm
	{0.000332,0.000120,0.000000}, // 750 nm
	{0.000235,0.000085,0.000000}, // 755 nm
	{0.000166,0.000060,0.000000}, // 760 nm
	{0.000117,0.000042,0.000000}, // 765 nm
	{0.000083,0.000030,0.000000}, // 770 nm
	{0.000059,0.000021,0.000000}, // 775 nm
	{0.000042,0.000015,0.000000}  // 780 nm
};

float cieXYZbins[SPEC_SAMPLES][3]; // Condensed from cieXYZ

double cieXbase = 0, cieYbase = 0, cieZbase = 0; // Integrals of [xyz]-bar

void cieXYZ_init()
{
	int i, j, n;

	for(i = 0; i < CIEXYZ_SAMPLES; i++)
		cieXbase += CIEXYZ_STEP*cieXYZ[i][0],
		cieYbase += CIEXYZ_STEP*cieXYZ[i][1],
		cieZbase += CIEXYZ_STEP*cieXYZ[i][2];

	for(i = (SPEC_START - CIEXYZ_START)/CIEXYZ_STEP, j = 0, n = 0;
		i < (SPEC_END - CIEXYZ_START)/CIEXYZ_STEP; i++)
	{
		cieXYZbins[j][0] += cieXYZ[i][0], cieXYZbins[j][1] += cieXYZ[i][1],
		cieXYZbins[j][2] += cieXYZ[i][2], n++;

		if(j < floor(((i*CIEXYZ_STEP - SPEC_START + CIEXYZ_START)/SPEC_STEP)))
			cieXYZbins[j][0] /= n, cieXYZbins[j][1] /= n,
			cieXYZbins[j][2] /= n, n = 0, j++;
	}
}

void rgb_to_color(color_t *c, float r, float g, float b)
{
	int i;

/*	if(r <= 0.04045) r /= 12.92;
	else if(r < 1) r = pow((r + 0.055)/1.055,2.4);
	if(g <= 0.04045) g /= 12.92;
	else if(g < 1) g = pow((g + 0.055)/1.055,2.4);
	if(b <= 0.04045) b /= 12.92;
	else if(b < 1) b = pow((b + 0.055)/1.055,2.4);*/

	for(i = 0; i < SPEC_SAMPLES; i++)
		c->w[i] = SPEC_START + i*SPEC_STEP,
		(c->i[i] =
			  c->w[i] <= 400 - SPEC_STEP ? 0
			: c->w[i] <  400 ? b*(c->w[i] - 400)/SPEC_STEP + 1
			: c->w[i] <= 500 - SPEC_STEP ? b
			: c->w[i] <  500 ? g + (b - g)*(500 - c->w[i])/SPEC_STEP
			: c->w[i] <= 600 - SPEC_STEP ? g
			: c->w[i] <  600 ? r + (g - r)*(600 - c->w[i])/SPEC_STEP
			: c->w[i] <= 700 - SPEC_STEP ? r
			: c->w[i] <  700 ? r*(700 - c->w[i])/SPEC_STEP
			: 0) < 0 ? c->i[i] = 0 : 0;
}

void color_to_XYZ(float *X, float *Y, float *Z, color_t *c)
{
	int i;
	float inten;
	*X = 0, *Y = 0, *Z = 0;
	for(i = (SPEC_START - CIEXYZ_START)/CIEXYZ_STEP;
		i < (SPEC_END - CIEXYZ_START)/CIEXYZ_STEP; i++)
		inten = CIEXYZ_STEP*c->i[(int)(i*(float)(CIEXYZ_STEP/SPEC_STEP)
			- (float)((SPEC_START - CIEXYZ_START)/SPEC_STEP))],
		*X += inten*cieXYZ[i][0],
		*Y += inten*cieXYZ[i][1],
		*Z += inten*cieXYZ[i][2];
}

void XYZ_to_rgb(float *r, float *g, float *b, float *X, float *Y, float *Z)
{
	*r = ( 3.2404542**X - 1.5371385**Y - 0.4985314**Z)/cieYbase;
	*g = (-0.9692660**X + 1.8760108**Y + 0.0415560**Z)/cieYbase;
	*b = ( 0.0556434**X - 0.2040259**Y + 1.0572252**Z)/cieYbase;
}

void color_to_rgb(float *r, float *g, float *b, color_t *c)
{
	float X, Y, Z;

	color_to_XYZ(&X,&Y,&Z,c);
	XYZ_to_rgb(r,g,b,&X,&Y,&Z);

/*	if(*r <= 0.0031308) *r *= 12.92;
	else *r = 1.055*pow(*r,1/2.4) - 0.055;
	if(*g <= 0.0031308) *g *= 12.92;
	else *g = 1.055*pow(*g,1/2.4) - 0.055;
	if(*b <= 0.0031308) *b *= 12.92;
	else *b = 1.055*pow(*b,1/2.4) - 0.055;*/
}

